#include "locater.h"
#include "pid.h"
#include "steering_wheel.h"
#include "FreeRTOS.h"
#include "task.h"
#include "queue.h"
#include "cmsis_os.h"
#include "can_rec.h"

float delta_x = 0.0f, delta_y = 0.0f;
locater_def locater = {0};
locater_def aim_point = {0};

uint8_t robot_point_now = 0;

typedef union
{
    uint8_t data_8[12];
    int32_t data_32[3];
    float data_f[3];
}unionf3;

void locater_data_rec(uint8_t *data, locater_def *loc)
{
    unionf3 union_loc;
    uint8_t i;
    if(data[0] == 'P' && data[1] == 'G')
    {
        heartbeat_fresh_locater();
        for(i = 0; i < 12; i++)
            union_loc.data_8[i] = data[i+2];
        loc->pos_x = union_loc.data_f[0] + delta_x;
        loc->pos_y = union_loc.data_f[1] + delta_y;
        loc->angle_last = loc->angle;
        loc->angle = union_loc.data_f[2];

        if(loc->angle - loc->angle_last > 180.0f)
            loc->round--;
        else if(loc->angle - loc->angle_last < -180.0f)
            loc->round++;
        loc->total_angle = loc->angle + loc->round * 360.0f;

    }
}
